/*
 * CarControl.c
 *
 *  Created on: Mar 9, 2011
 *      Author: wli3
 * Feature Spec:
 * 1. Support Servo1 & Servo2 turn to Position Number.
 * 2. Support Servo1 & Servo2 turn to high Position and down.
 * 3. Support the car move forward, backword, turn left & turn right.
 * 4. Support Serial Control & IR Control.
 * 5. In Serial Control, this Function will be allowed:
 *    Camera move UP, Down, Left, Right.(4 arguments, {101-104})
 *    Camera turn to position S1Px-S2Px.(S1Px{S1P1-S1P9}-S2Px{S2P1-S2P9}, 81 arguments{211-255})
 *    Car move to Forward, Backward, Left, Right, Stop.(5 arguments, {301-305})
 *    Car step move to Forward, Backward, Left, Right, with step code.(40 arguments, {310-349})
 *
 */

#include <Servo.h>
#define TIMER_RESET TCNT1 = 0
#define SAMPLE_SIZE 68
#define DEFAULT_SPEED 250

//--------Const Define------
//Servo angle
const int CAMERA_POSITION_SERVO1_ANGLES[9] = { 60, 75, 90, 105, 120, 135, 150,
		165, 180 };
const int CAMERA_POSITION_SERVO2_ANGLES[9] = { 150, 135, 120, 105, 90, 75, 60,
		45, 30 };
const int CAR_STEP_FRONT_AND_BACK_MILLISECOND[10] = { 500, 1000, 1500, 2000,
		2500, 3000, 3500, 4000, 4500, 5000 };
const int CAR_STEP_LEFT_AND_RIGHT_MILLISECOND[10] = { 100, 200, 300, 400, 500,
		600, 700, 800, 900, 1000 };

const int CAMERA_POSITION_SERVO1_ANGLE_DEFAULT = 4;
const int CAMERA_POSITION_SERVO2_ANGLE_DEFAULT = 4;

//IR Code
const unsigned long IR_CODE_CAMERA_POSITION_UL = 59903;
const unsigned long IR_CODE_CAMERA_POSITION_UM = 55823;
const unsigned long IR_CODE_CAMERA_POSITION_UR = 63983;
const unsigned long IR_CODE_CAMERA_POSITION_ML = 53783;
const unsigned long IR_CODE_CAMERA_POSITION_MM = 61943;
const unsigned long IR_CODE_CAMERA_POSITION_MR = 57863;
const unsigned long IR_CODE_CAMERA_POSITION_DL = 66023;
const unsigned long IR_CODE_CAMERA_POSITION_DM = 52763;
const unsigned long IR_CODE_CAMERA_POSITION_DR = 60923;
const unsigned long IR_CODE_CAMERA_TURN_LEFT = 67553;
const unsigned long IR_CODE_CAMERA_TURN_RIGHT = 65003;
const unsigned long IR_CODE_CAMERA_TURN_UP = 63601;
const unsigned long IR_CODE_CAMERA_TURN_DOWN = 61051;
const unsigned long IR_CODE_CAR_MOVE_FORWARD = 61433;
const unsigned long IR_CODE_CAR_MOVE_BACKWARD = 63473;
const unsigned long IR_CODE_CAR_MOVE_LEFT = 53401;
const unsigned long IR_CODE_CAR_MOVE_RIGHT = 57481;
const unsigned long IR_CODE_CAR_MOVE_STAND = 61561;

//Wireless Controler pulse areas
const int WC_AREA_CHANNEL1_HIGH[2] = { 900, 1200 };
const int WC_AREA_CHANNEL1_LOW[2] = { 1800, 2100 };
const int WC_AREA_CHANNEL2_HIGH[2] = { 900, 1200 };
const int WC_AREA_CHANNEL2_LOW[2] = { 1800, 2100 };
const int WC_AREA_CHANNEL3_HIGH[2] = { 900, 1200 };
const int WC_AREA_CHANNEL3_LOW[2] = { 1800, 2100 };
const int WC_AREA_CHANNEL4_HIGH[2] = { 900, 1200 };
const int WC_AREA_CHANNEL4_LOW[2] = { 1800, 2100 };

const int WC_AREA_CHANNEL5_ENABLE[2] = { 1800, 2100 };
const int WC_AREA_CHANNEL6_AVAILABLE[2] = { 900, 2100 };

//--------Pin Define--------
//Driver, include I1,I2,I3,I4,EA,EB
int i1Pin = 26;
int i2Pin = 27;
int i3Pin = 28;
int i4Pin = 29;
int speedPinA = 6;
int speedPinB = 7;

//Buzzer
int BUZZER = 45;

//IR Receiver
int irPin = 44;

//Servo
int servoPin1 = 22;
int servoPin2 = 23;
Servo myServo1;
Servo myServo2;

//Wireless Controler
int wcPin1 = 21;
int wcPin2 = 20;
int wcPin3 = 19;
int wcPin4 = 18;
int wcPin5 = 17;
int wcPin6 = 16;

//--------Argument Define--------
//IR arguments
unsigned long irc;
unsigned long data;
unsigned int TimerValue[SAMPLE_SIZE];
char direction[SAMPLE_SIZE];
byte change_count;
long time1, time2;

//Servo status
int servoStatus1 = CAMERA_POSITION_SERVO1_ANGLE_DEFAULT;
int servoStatus2 = CAMERA_POSITION_SERVO2_ANGLE_DEFAULT;
int pulseWidth;

//Serial arguments
int serialCode;
char commandLine[100];

//Wireless Control pulse
int channelPulse1;
int channelPulse2;
int channelPulse3;
int channelPulse4;
int channelPulse5;
int channelPulse6;

void setup() {

	//Open Serial
	Serial.begin(9600);
	Serial.println("Start~~~");

	//Driver Pin, I1-I4 is digital signal
	pinMode(i1Pin, OUTPUT);
	pinMode(i2Pin, OUTPUT);
	pinMode(i3Pin, OUTPUT);
	pinMode(i4Pin, OUTPUT);

	//EA & EB is PWM signal, and output format with duty cycle
	pinMode(speedPinA, OUTPUT);
	pinMode(speedPinB, OUTPUT);
	analogWrite(speedPinA, DEFAULT_SPEED);
	analogWrite(speedPinB, DEFAULT_SPEED);

	//Servo initial
	myServo1.attach(servoPin1);
	myServo2.attach(servoPin2);
	//myServo1.write(CAMERA_POSITION_SERVO1_ANGLES[CAMERA_POSITION_SERVO1_ANGLE_DEFAULT]);
	//myServo2.write(CAMERA_POSITION_SERVO2_ANGLES[CAMERA_POSITION_SERVO2_ANGLE_DEFAULT]);
	//pinMode(servoPin1, OUTPUT);
	//pinMode(servoPin2, OUTPUT);
	//servoPulse(servoPin1, CAMERA_POSITION_SERVO1_ANGLES[CAMERA_POSITION_SERVO1_ANGLE_DEFAULT]);
	//servoPulse(servoPin2, CAMERA_POSITION_SERVO2_ANGLES[CAMERA_POSITION_SERVO2_ANGLE_DEFAULT]);

	int servo1Angle = myServo1.read();
	int servo2Angle = myServo2.read();
	Serial.print(", Servo1 angle is: ");
	Serial.print(servo1Angle);
	Serial.print(", Servo2 angle is: ");
	Serial.print(servo2Angle);

	//Buzzer Pin
	pinMode(BUZZER, OUTPUT);

	//IR Pin
	pinMode(irPin, INPUT);

	//IR arguments
	TCCR1B = 0x03;
	TCCR1A = 0x00;

	//Wireless Control
	pinMode(wcPin1, INPUT);
	pinMode(wcPin2, INPUT);
	pinMode(wcPin3, INPUT);
	pinMode(wcPin4, INPUT);
	pinMode(wcPin5, INPUT);
	pinMode(wcPin6, INPUT);

	//Bee
	Serial.println(".");
	bee();
}

void loop() {
	if (!checkChannel5Enable()) {
		handleSerial();
		return;
	}
	handleWirelessControl();

	//while (digitalRead(irPin) == HIGH) {
	//handleSerial();
	//}
	//handleIr();
}

//----LOGIC, TOP Level for Serial, WirelessControl, IR----

void handleSerial() {
	boolean isAvailable = readSerialLine();
	if (!isAvailable)
		return;
	Serial.print("Command is: ");
	Serial.print((char*) commandLine);
	handleCommandLine();
	Serial.println(".");
	bee();
}

void handleWirelessControl() {
	//Servo1
	handleChannel2();
	//Servo2
	handleChannel4();
	//Speed Control
	handleChannel6();
	//Move Control
	handleChannel31();
}

void handleIr() {
	getIrCode();
	Serial.print("IR code is: ");
	Serial.print(irc);
	if (!mappingIrCodeToCommandLine()) {
		Serial.println(".");
		return;
	}
	Serial.print(", Command is: ");
	Serial.print((char*) commandLine);
	handleCommandLine();
	Serial.println(".");
	bee();
}

//----LOGIC, Mapping IR Code to Command Line----

boolean mappingIrCodeToCommandLine() {
	boolean isMaped = true;
	irc = irc % 100000;
	if (irc == IR_CODE_CAMERA_POSITION_UL) {
		setCommandLine(251);
	} else if (irc == IR_CODE_CAMERA_POSITION_UM) {
		setCommandLine(253);
	} else if (irc == IR_CODE_CAMERA_POSITION_UR) {
		setCommandLine(255);
	} else if (irc == IR_CODE_CAMERA_POSITION_ML) {
		setCommandLine(231);
	} else if (irc == IR_CODE_CAMERA_POSITION_MM) {
		setCommandLine(233);
	} else if (irc == IR_CODE_CAMERA_POSITION_MR) {
		setCommandLine(235);
	} else if (irc == IR_CODE_CAMERA_POSITION_DL) {
		setCommandLine(211);
	} else if (irc == IR_CODE_CAMERA_POSITION_DM) {
		setCommandLine(213);
	} else if (irc == IR_CODE_CAMERA_POSITION_DR) {
		setCommandLine(215);
	} else if (irc == IR_CODE_CAMERA_TURN_LEFT) {
		setCommandLine(101);
	} else if (irc == IR_CODE_CAMERA_TURN_RIGHT) {
		setCommandLine(102);
	} else if (irc == IR_CODE_CAMERA_TURN_UP) {
		setCommandLine(103);
	} else if (irc == IR_CODE_CAMERA_TURN_DOWN) {
		setCommandLine(104);
	} else if (irc == IR_CODE_CAR_MOVE_FORWARD) {
		setCommandLine(301);
	} else if (irc == IR_CODE_CAR_MOVE_BACKWARD) {
		setCommandLine(302);
	} else if (irc == IR_CODE_CAR_MOVE_LEFT) {
		setCommandLine(303);
	} else if (irc == IR_CODE_CAR_MOVE_RIGHT) {
		setCommandLine(304);
	} else if (irc == IR_CODE_CAR_MOVE_STAND) {
		setCommandLine(305);
	} else {
		isMaped = false;
	}
	return isMaped;
}

//----LOGIC, Dispatch Command to 3 Types----

void handleCommandLine() {
	if (commandLine[0] == '1') {
		if (commandLine[1] == '0' && commandLine[2] >= '1' && commandLine[2]
				<= '4') {
			handleCameraTurn(commandLine[2]);
		}
	} else if (commandLine[0] == '2') {
		if (commandLine[1] >= '1' && commandLine[1] <= '9' && commandLine[2]
				>= '1' && commandLine[2] <= '9') {
			servoStatus1 = commandLine[1] - '1';
			servoStatus2 = commandLine[2] - '1';
			handleCameraPosition();
		}
	} else if (commandLine[0] == '3') {
		if (commandLine[1] == '0' && commandLine[2] >= '1' && commandLine[2]
				<= '5') {
			handleCarMove(commandLine[2]);
		}
		if (commandLine[1] >= '1' && commandLine[1] <= '4' && commandLine[2]
				>= '0' && commandLine[2] <= '9') {
			handleCarMoveStep(commandLine[1], commandLine[2]);
		}
	}
}

//----INPUT & LOGIC, Wireless Control----

void handleChannel2() {//servo1
	int readPulse = pulseIn(wcPin2, HIGH);
	if (checkInArea(readPulse, WC_AREA_CHANNEL2_HIGH)) {
		handleCameraTurn('1');//turn up
		delay(50);
	} else if (checkInArea(readPulse, WC_AREA_CHANNEL2_LOW)) {
		handleCameraTurn('2');//turn down
		delay(50);
	}
	channelPulse2 = readPulse;
}

void handleChannel4() {//servo2
	int readPulse = pulseIn(wcPin4, HIGH);
	if (checkInArea(readPulse, WC_AREA_CHANNEL4_HIGH)) {
		handleCameraTurn('3');//turn left
		delay(50);
	} else if (checkInArea(readPulse, WC_AREA_CHANNEL4_LOW)) {
		handleCameraTurn('4');//turn right
		delay(50);
	}
	channelPulse4 = readPulse;
}

void handleChannel6() {//car speed
	int readPulse = pulseIn(wcPin6, HIGH);
	int speed = DEFAULT_SPEED;
	if (checkInArea(readPulse, WC_AREA_CHANNEL6_AVAILABLE)) {
		speed = (readPulse - 900) / 5;
	}
	analogWrite(speedPinA, speed);
	analogWrite(speedPinB, speed);
	channelPulse6 = readPulse;
}

void handleChannel31() {//car move
	int readPulse = pulseIn(wcPin3, HIGH);
	if (checkInArea(readPulse, WC_AREA_CHANNEL3_HIGH)) {
		handleCarMove('1');//car move forward
	} else if (checkInArea(readPulse, WC_AREA_CHANNEL3_LOW)) {
		handleCarMove('2');//car move backward
	} else {
		handleChannel1();
	}
	channelPulse3 = readPulse;
	delay(10);
}

void handleChannel1() {//car turn left & right
	int readPulse = pulseIn(wcPin1, HIGH);
	if (checkInArea(readPulse, WC_AREA_CHANNEL1_HIGH)) {
		handleCarMove('3');//car turn left
	} else if (checkInArea(readPulse, WC_AREA_CHANNEL1_LOW)) {
		handleCarMove('4');//car turn right
	} else {
		handleCarMove('5');//car stand
	}
	channelPulse1 = readPulse;
}

//----LOGIC, Camera Turn----

void handleCameraTurn(char actionCode) {
	if (actionCode == '1') {
		if (servoStatus1 > 0) {
			Serial.print(", Camera turn up");
			servoStatus1 -= 1;
			handleCameraPosition();
		}
	} else if (actionCode == '2') {
		if (servoStatus1 < 8) {
			Serial.print(", Camera turn down");
			servoStatus1 += 1;
			handleCameraPosition();
		}
	} else if (actionCode == '3') {
		if (servoStatus2 > 0) {
			Serial.print(", Camera turn left");
			servoStatus2 -= 1;
			handleCameraPosition();
		}
	} else if (actionCode == '4') {
		if (servoStatus2 < 8) {
			Serial.print(", Camera turn right");
			servoStatus2 += 1;
			handleCameraPosition();
		}
	}
}

//----LOGIC, Car Basic Move----

void handleCarMove(char actionCode) {
	if (actionCode == '1') {
		Serial.print(", Car move forward");
		forward();
	} else if (actionCode == '2') {
		Serial.print(", Car move backward");
		backward();
	} else if (actionCode == '3') {
		Serial.print(", Car turn left");
		left();
	} else if (actionCode == '4') {
		Serial.print(", Car turn right");
		right();
	} else if (actionCode == '5') {
		Serial.print(", Car stand");
		stand();
	}
}

//----LOGIC, Car Step Move----

void handleCarMoveStep(char actionCode, char stepCode) {
	if (actionCode == '1') {
		Serial.print(", Car step move forward");
		forward();
		handleFrontAndBackStep(stepCode);
		stand();
	} else if (actionCode == '2') {
		Serial.print(", Car step move backward");
		backward();
		handleFrontAndBackStep(stepCode);
		stand();
	} else if (actionCode == '3') {
		Serial.print(", Car step turn left");
		left();
		handleLeftAndRightStep(stepCode);
		stand();
	} else if (actionCode == '4') {
		Serial.print(", Car step turn right");
		right();
		handleLeftAndRightStep(stepCode);
		stand();
	}
}

void handleLeftAndRightStep(char stepCode) {
	Serial.print(", Step Code: ");
	Serial.print(stepCode);
	Serial.print(", Step Time: ");
	Serial.print(CAR_STEP_LEFT_AND_RIGHT_MILLISECOND[stepCode - '0']);
	delay(CAR_STEP_LEFT_AND_RIGHT_MILLISECOND[stepCode - '0']);
}

void handleFrontAndBackStep(char stepCode) {
	Serial.print(", Step Code: ");
	Serial.print(stepCode);
	Serial.print(", Step Time: ");
	Serial.print(CAR_STEP_FRONT_AND_BACK_MILLISECOND[stepCode - '0']);
	delay(CAR_STEP_FRONT_AND_BACK_MILLISECOND[stepCode - '0']);
}

//----INPUT, Serial Read----

boolean readSerialLine() {
	int i = 0;
	char temp;
	if (Serial.available()) {
		delay(100);
		while (Serial.available() && i < 99) {
			temp = Serial.read();
			if (temp == '\0')
				break;
			commandLine[i++] = temp;
		}
		commandLine[i++] = '\0';
	}

	if (i > 0) {
		return true;
	}
	return false;
}

//----INPUT, IR Read----

void getIrCode() {
	TIMER_RESET;
	change_count = 0;
	TimerValue[change_count] = TCNT1;
	char temp = '0';
	direction[change_count++] = '0';
	while (change_count < SAMPLE_SIZE) {
		if (temp == '0') {
			//Serial.println("enter low");
			int i = 0;
			while (digitalRead(irPin) == LOW) {
				i++;
				if (i > 6000)
					break;
			} //Wait for level signal turn down

			//Serial.println(i);
			TimerValue[change_count] = TCNT1;
			temp = '1';
			direction[change_count++] = '1'; //keep signal
		} else {
			//Serial.println("enter high");
			int i = 0;
			while (digitalRead(irPin) == HIGH) {
				i++;
				if (i > 6000)
					break;
			} //Wait for level signal turn down

			//Serial.println(i);
			TimerValue[change_count] = TCNT1;
			temp = '0';
			direction[change_count++] = '0'; //keep signalA
		}
	}
	change_count = 0;
	data = 0;
	time1 = (long) TimerValue[change_count] * 4;
	change_count += 2;
	while (change_count < SAMPLE_SIZE) {
		time2 = (long) TimerValue[change_count] * 4;
		if ((time2 - time1) > 8000) {
		} else if ((time2 - time1) > 1680) {
			data = data << 1;
			data |= 1;
		} else {
			data = data << 1;
		}
		change_count += 2;
		time1 = time2;
	}
	data = data >> 8;
	irc = data;
}

//----OUTPUT, Car Function----

void stand() {
	analogWrite(speedPinA, DEFAULT_SPEED);
	analogWrite(speedPinB, DEFAULT_SPEED);
	digitalWrite(i1Pin, false);
	digitalWrite(i2Pin, false);
	digitalWrite(i3Pin, false);
	digitalWrite(i4Pin, false);
}

void backward() {
	analogWrite(speedPinA, DEFAULT_SPEED);
	analogWrite(speedPinB, DEFAULT_SPEED);
	digitalWrite(i1Pin, false);
	digitalWrite(i2Pin, true);
	digitalWrite(i3Pin, false);
	digitalWrite(i4Pin, true);
}

void forward() {
	analogWrite(speedPinA, DEFAULT_SPEED);
	analogWrite(speedPinB, DEFAULT_SPEED);
	digitalWrite(i1Pin, true);
	digitalWrite(i2Pin, false);
	digitalWrite(i3Pin, true);
	digitalWrite(i4Pin, false);
}

void right() {
	analogWrite(speedPinA, DEFAULT_SPEED);
	analogWrite(speedPinB, DEFAULT_SPEED);
	digitalWrite(i1Pin, false);
	digitalWrite(i2Pin, true);
	digitalWrite(i3Pin, true);
	digitalWrite(i4Pin, false);
}

void left() {
	analogWrite(speedPinA, DEFAULT_SPEED);
	analogWrite(speedPinB, DEFAULT_SPEED);
	digitalWrite(i1Pin, true);
	digitalWrite(i2Pin, false);
	digitalWrite(i3Pin, false);
	digitalWrite(i4Pin, true);
}

//----OUTPUT, Servo Function----

void handleCameraPosition() {
	Serial.print(", Servo1's position is: ");
	Serial.print(servoStatus1);
	Serial.print(", Servo2's position is: ");
	Serial.print(servoStatus2);
	myServo1.write(CAMERA_POSITION_SERVO1_ANGLES[servoStatus1]);
	myServo2.write(CAMERA_POSITION_SERVO2_ANGLES[servoStatus2]);
	//servoPulse(servoPin1, CAMERA_POSITION_SERVO1_ANGLES[servoStatus1]);
	//servoPulse(servoPin2, CAMERA_POSITION_SERVO2_ANGLES[servoStatus2]);
}

void servoPulse(int servopin, int myangle) {//Servo control
	pulseWidth = (myangle * 11) + 500;//Count angle to Pulse
	for (int i = 0; i <= 50; i++) {
		digitalWrite(servopin, HIGH);//Level signal turn Up
		delayMicroseconds(pulseWidth);//Keep Pulse time
		digitalWrite(servopin, LOW);//Level signal turn down
		delayMicroseconds(20000 - pulseWidth);
	}
}

//----OUTPUT, Buzz Function----

void bee() {
	digitalWrite(BUZZER, LOW);
	delay(300);
	digitalWrite(BUZZER, HIGH);
	delay(300);
}

//----Utilities----

boolean checkChannel5Enable() {
	channelPulse5 = pulseIn(wcPin5, HIGH);
	return checkInArea(channelPulse5, WC_AREA_CHANNEL5_ENABLE);
}

boolean checkInArea(int pulse, const int area[]) {
	if (pulse > area[0] && pulse < area[1]) {
		return true;
	}
	return false;
}

boolean isPluseChangeToTheArea(int newPulse, int oldPulse, const int area[]) {
	if (checkInArea(newPulse, area) && !checkInArea(oldPulse, area)) {
		return true;
	}
	return false;
}

void setCommandLine(int command) {
	Serial.println("DEBUG-gamiao11");
	for (int i = 2; i >= 0; i--) {
		Serial.println("DEBUG-gamiao12");
		commandLine[i] = '0' + command % 10;
		command = command / 10;
	}
	commandLine[3] = '\0';
	Serial.println("DEBUG-gamiao13");
}
